#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
from controller_manager_msgs.srv import (
    ListHardwareComponents,
    SetHardwareComponentState,
)
import tkinter as tk
from tkinter import ttk
import threading

joint_limits = [
    (0.01, 0.15),
    (0.01, 0.21),
    (0.01, 0.14),
    (0, 3.14),
    (-1.57, 1.57),
    (-1.57, 1.57),
    (-100, 100),
]

joint_names = [
    "Lift",
    "Stretch",
    "Shift",
    "Arm Joint 1",
    "Arm Joint 2",
    "Arm Joint 3",
    "Suck",
]


class PositionController(Node):
    def __init__(self):
        super().__init__("position_controller_gui")
        self.publisher = self.create_publisher(
            Float64MultiArray, "/joint_group_position_controller/commands", 10
        )

        # Hardware management clients
        self.list_hardware_client = self.create_client(
            ListHardwareComponents, "/controller_manager/list_hardware_components"
        )
        self.set_hardware_state_client = self.create_client(
            SetHardwareComponentState,
            "/controller_manager/set_hardware_component_state",
        )

        self.root = tk.Tk()
        self.root.title("Joint Position Controller")
        self.root.geometry("600x550")
        self.root.resizable(True, True)

        self.positions = [0.0] * 7
        self.sliders = []
        self.value_labels = []
        self.hardware_active = False

        # Create main frame
        main_frame = ttk.Frame(self.root)
        main_frame.pack(fill=tk.BOTH, expand=True, padx=10, pady=10)

        # Hardware control frame
        control_frame = ttk.LabelFrame(
            main_frame, text="Hardware Control", padding="10"
        )
        control_frame.pack(fill=tk.X, pady=(0, 10))

        # Control buttons
        button_frame = ttk.Frame(control_frame)
        button_frame.pack(fill=tk.X)

        self.activate_button = ttk.Button(
            button_frame,
            text="Activate Hardware",
            command=self.activate_hardware,
            width=18,
        )
        self.activate_button.pack(side=tk.LEFT, padx=(0, 10))

        self.deactivate_button = ttk.Button(
            button_frame,
            text="Deactivate Hardware",
            command=self.deactivate_hardware,
            width=18,
        )
        self.deactivate_button.pack(side=tk.LEFT, padx=(0, 10))

        self.refresh_button = ttk.Button(
            button_frame,
            text="Refresh Status",
            command=self.refresh_hardware_status,
            width=15,
        )
        self.refresh_button.pack(side=tk.LEFT, padx=(0, 10))

        # Status display
        self.status_label = ttk.Label(
            control_frame, text="Status: Checking...", font=("Arial", 10, "bold")
        )
        self.status_label.pack(pady=(10, 0))

        # Joint control frame
        joint_frame = ttk.LabelFrame(main_frame, text="Joint Control", padding="10")
        joint_frame.pack(fill=tk.BOTH, expand=True)

        # Create sliders
        for i in range(7):
            frame = ttk.Frame(joint_frame)
            frame.pack(fill=tk.X, pady=5)

            # Joint name label
            name_label = ttk.Label(
                frame, text=f"{joint_names[i]}:", width=12, anchor="w"
            )
            name_label.pack(side=tk.LEFT, padx=(0, 10))

            min_val, max_val = joint_limits[i]

            # Slider
            slider = ttk.Scale(
                frame,
                from_=min_val,
                to=max_val,
                orient=tk.HORIZONTAL,
                length=300,
                command=self.make_update_callback(i),
                state="disabled",
            )
            slider.pack(side=tk.LEFT, padx=(0, 10))
            self.sliders.append(slider)

            # Value display
            value_label = ttk.Label(
                frame, text=f"{min_val:.3f}", width=10, anchor="center"
            )
            value_label.pack(side=tk.LEFT, padx=(0, 5))
            self.value_labels.append(value_label)

            # Range label
            range_label = ttk.Label(
                frame,
                text=f"[{min_val:.2f}, {max_val:.2f}]",
                font=("Arial", 8),
                foreground="gray",
            )
            range_label.pack(side=tk.LEFT)

        # Initial hardware status check
        self.root.after(1000, self.refresh_hardware_status)
        self.schedule_next_publish()

    def activate_hardware(self):
        """Activate hardware component"""
        self.get_logger().info("Activating hardware...")
        if not self.set_hardware_state_client.wait_for_service(timeout_sec=2.0):
            self.get_logger().error("Hardware state service not available")
            return

        request = SetHardwareComponentState.Request()
        request.name = "xeno_control"
        request.target_state.id = 3  # ACTIVE state
        request.target_state.label = "active"

        future = self.set_hardware_state_client.call_async(request)
        future.add_done_callback(self.handle_hardware_state_response)

    def deactivate_hardware(self):
        """Deactivate hardware component"""
        self.get_logger().info("Deactivating hardware...")
        if not self.set_hardware_state_client.wait_for_service(timeout_sec=2.0):
            self.get_logger().error("Hardware state service not available")
            return

        request = SetHardwareComponentState.Request()
        request.name = "xeno_control"
        request.target_state.id = 2  # INACTIVE state
        request.target_state.label = "inactive"

        future = self.set_hardware_state_client.call_async(request)
        future.add_done_callback(self.handle_hardware_state_response)

    def handle_hardware_state_response(self, future):
        """Handle hardware state change response"""
        try:
            response = future.result()
            if response.ok:
                self.get_logger().info("Hardware state changed successfully")
                # Refresh status after a short delay
                self.root.after(500, self.refresh_hardware_status)
            else:
                self.get_logger().error("Failed to change hardware state")
        except Exception as e:
            self.get_logger().error(f"Hardware state service call failed: {e}")

    def refresh_hardware_status(self):
        """Get current hardware status"""
        if not self.list_hardware_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().warn("Hardware list service not available")
            return

        request = ListHardwareComponents.Request()
        future = self.list_hardware_client.call_async(request)
        future.add_done_callback(self.handle_hardware_status_response)

    def handle_hardware_status_response(self, future):
        """Handle hardware status response"""
        try:
            response = future.result()
            xeno_hardware = None

            for component in response.component:
                if component.name == "xeno_control":
                    xeno_hardware = component
                    break

            if xeno_hardware:
                is_active = xeno_hardware.state.label == "active"
                self.hardware_active = is_active

                # Update UI on main thread
                self.root.after(
                    0,
                    lambda: self.update_hardware_status(
                        is_active, xeno_hardware.state.label
                    ),
                )
            else:
                self.get_logger().warn("xeno_control hardware component not found")

        except Exception as e:
            self.get_logger().error(f"Hardware status service call failed: {e}")

    def update_hardware_status(self, is_active, state_label):
        """Update UI based on hardware status"""
        if is_active:
            self.status_label.config(
                text=f"Status: {state_label.capitalize()}", foreground="green"
            )
            self.activate_button.config(state="disabled")
            self.deactivate_button.config(state="normal")

            # Enable sliders
            for slider in self.sliders:
                slider.config(state="normal")
        else:
            self.status_label.config(
                text=f"Status: {state_label.capitalize()}", foreground="red"
            )
            self.activate_button.config(state="normal")
            self.deactivate_button.config(state="disabled")

            # Disable sliders
            for slider in self.sliders:
                slider.config(state="disabled")

    def make_update_callback(self, joint_idx):
        """Create callback for slider updates"""

        def callback(value):
            if self.hardware_active:
                self.update_position(joint_idx, value)
            self.value_labels[joint_idx].config(text=f"{float(value):.3f}")

        return callback

    def update_position(self, joint_idx, value):
        self.positions[joint_idx] = float(value)
        # self.get_logger().info(f"Updated {joint_names[joint_idx]} to {value}")

    def publish_positions(self):
        if self.hardware_active:
            msg = Float64MultiArray()
            msg.data = self.positions
            self.publisher.publish(msg)

    def schedule_next_publish(self):
        """Schedule next publish"""
        self.publish_positions()
        self.root.after(100, self.schedule_next_publish)

    def run(self):
        self.ros_thread = threading.Thread(target=self.spin_ros, daemon=True)
        self.ros_thread.start()

        try:
            self.root.mainloop()
        finally:
            if rclpy.ok():
                self.destroy_node()

    def spin_ros(self):
        """Spin ROS2 in separate thread"""
        try:
            rclpy.spin(self)
        except Exception:
            pass


def main():
    rclpy.init()
    controller = PositionController()

    try:
        controller.run()
    except KeyboardInterrupt:
        pass
    finally:
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == "__main__":
    main()
